#include "Skeleton.h"
#include "TRSTransform.h"
#include "RotationControllerNode.h"
#include "RotationController.h"

#include <memory.h>
#include <windows.h>

Skeleton::Skeleton(void):
	m_World(new TRSTransform()),
	m_pAnimation(NULL),
	m_BoneMatrices(NULL),
	m_DebugVisualCreated(false),
	m_AnimationStart(0),
	m_AnimationTime(0),
	m_LocalTransformsCreated(false)
{
	m_BoneMatrices = new float[16*40];//m_NodeMap.size()];
	
	DAEMatrix id;
	for (int i =1;i<=40;i++)
		this->SetBoneMatrix(i,id);
}

Skeleton::~Skeleton(void)
{
	map<tstring,SkeletonNode*>::const_iterator it;
	for ( it = m_NodeMap.begin() ; it != m_NodeMap.end(); ++it)
	{
		delete (*it).second;
	}
	/*
	map<tstring,>::const_iterator ait;
	for (ait= m_Animations.begin(); ait != m_Animations.end(); ++ait){
		delete (*ait).second;
	}*/
	delete m_World;
	if ( m_BoneMatrices != NULL )
		delete[] m_BoneMatrices;
	m_pAnimation = NULL;
}

void Skeleton::AddRoot(SkeletonNode* node)
{
	m_RootNodes.push_back(node);
	AddNodeChildren(node);
	
}

void Skeleton::AddNodeChildren(SkeletonNode* node)
{
	tstring name = node->GetName();

	//this->SetBoneMatrix(node->GetID(), node->GetInitialPose());
	map<tstring,SkeletonNode*>::iterator itOld = m_NodeMap.find(name);

	if( itOld != m_NodeMap.end() )
	{
		 SkeletonNode* oldNode = itOld->second;

		 if( oldNode != node )
		 {
				delete itOld->second;
		 }
	}

	m_NodeMap[name]=node;

	vector<SkeletonNode*>::const_iterator it;
	for (it = node->GetChildNodeBegin(); it != node->GetChildNodeEnd();++it)
	{
		AddNodeChildren(*it);
	}
}

void Skeleton::Animate(float time)
{
	if (m_pAnimation == NULL )
		return;
	if ( m_Looping && m_AnimationTime < m_pAnimation->GetStartFrame() )
		m_AnimationTime = (float)m_pAnimation->GetStartFrame();
	m_AnimationTime += time;
	if ( m_Looping && m_AnimationTime >= m_pAnimation->GetStopFrame() )
		m_AnimationTime = (float)m_pAnimation->GetStartFrame();
	vector<SkeletonNode*>::const_iterator it;
	DAEMatrix identity;
	for (it = m_RootNodes.begin();it!=m_RootNodes.end();++it)
	{
		SkeletonNode* child =*it;
		child->Animate(this, identity,  m_AnimationTime);
	}
}

void Skeleton::SetAnimation(Animation* animation)
{
	// calculate local transforms if necessary
	if ( !m_LocalTransformsCreated )
	{
		CalculateLocalTransformations();
		m_LocalTransformsCreated = false;
	}

	// clear all animations first to be sure.
	map<tstring,SkeletonNode*>::const_iterator nodeIt;
	for (nodeIt = m_NodeMap.begin(); nodeIt!=m_NodeMap.end();++nodeIt)
	{
		(*nodeIt).second->SetAnimationNode(NULL);
	}

	// set the new animations
	m_pAnimation = animation;
	AnimIT it;
	for (it = m_pAnimation->AnimationBegin();
		it!= m_pAnimation->AnimationEnd();
		++it)
	{
		tstring key = (*it).first;
		AnimationNode* an =(*it).second;
		map<tstring,SkeletonNode*>::const_iterator it;
		nodeIt = m_NodeMap.find(key);
		if (nodeIt != m_NodeMap.end()){
			SkeletonNode* sn = (*nodeIt).second;
			sn->SetAnimationNode(an);
		}
	}
	m_AnimationStart = timeGetTime();
}

void Skeleton::AddAnimation(Animation* animation){
	m_Animations[animation->GetName()]=AnimationPtr(animation);
}

void Skeleton::AddAnimation(AnimationPtr animation){
	m_Animations[animation->GetName()]=animation;
}

void Skeleton::ActivateAnimation(tstring name, bool looping)
{
	m_Looping = looping;
	if ( m_pAnimation!=NULL && name == m_pAnimation->GetName() )
		return;
	if (m_Animations.find(name) != m_Animations.end()){
		AnimationPtr animation = m_Animations[name];
		m_AnimationTime = (float)animation.get()->GetStartFrame();
		SetAnimation(animation.get());
	}
}

void Skeleton::SetBoneMatrix(int id,DAEMatrix& matrix)
{
	if (id>0){
		float* dest = m_BoneMatrices+ 16*(id-1);
		memcpy_s(dest,16*sizeof(float),matrix.GetValuePointer(),matrix.GetSizeInBytes());
	}
}

Skeleton* Skeleton::Clone() const
{
	Skeleton* pSkeleton = new Skeleton();

	tstring location(m_FileLocation.begin(),m_FileLocation.end());
	pSkeleton->SetFileLocation(location);
	map<tstring,SkeletonNode*>::const_iterator it;
	for ( it = m_NodeMap.begin() ;  it != m_NodeMap.end(); ++it)
	{
		tstring key = (*it).first;
		SkeletonNode * toCopy = (*it).second;
		SkeletonNode * clone = toCopy->Clone();
		if ( clone->GetParent() == NULL )
			pSkeleton->m_RootNodes.push_back(clone);
		pSkeleton->m_NodeMap[key]=clone;
	}
	// animations are shared
	map<tstring,AnimationPtr>::const_iterator animIT;
	for ( animIT = m_Animations.begin(); animIT != m_Animations.end(); ++animIT )
	{
		tstring key = (*animIT).first;
		AnimationPtr animation = (*animIT).second;
		pSkeleton->m_Animations[key]=animation;
	}

	return pSkeleton;
}

void Skeleton::CalculateLocalTransformations()
{
	/*
	map<tstring,SkeletonNode*>::const_iterator it;
	for ( it = m_NodeMap.begin() ;  it != m_NodeMap.end(); ++it)
	{
		SkeletonNode* node = (*it).second;
		node->CalculateLocalTransform();
	}
	*/
}

SkeletonNode* Skeleton::GetSkeletonNode(tstring name)
{
	map<tstring,SkeletonNode*>::const_iterator it;
	it = m_NodeMap.find(name);
	if (it != m_NodeMap.end() )
		return (*it).second;
	else
		return NULL;
}

Animation* Skeleton::CopyInitialPoseToAnimation()
{
	Animation* initialPose = new Animation(L"initialpose",0,1);
	map<tstring,SkeletonNode*>::const_iterator it;
	for ( it = m_NodeMap.begin() ;  it != m_NodeMap.end(); ++it)
	{
		SkeletonNode* node = (*it).second;
		RotationControllerNode* nodeCopy = new RotationControllerNode(node->GetName());
		DAEFloat3 rotation;
		node->GetRotation(rotation);
		DAEFloat3 translation;
		node->GetTranslation(translation);
		DAEFloat3 scale;
		node->GetScale(scale);
		nodeCopy->SetTranslation(translation);
		nodeCopy->SetScale(scale);
		nodeCopy->GetRotationController()->SetConstantRotation(rotation);
		initialPose->AddAnimationNode(nodeCopy);
	}
	return initialPose;
}